#if 0
#include <ros/ros.h>
#include <chrono>
#include <iostream>
#include <string>
#include <vector>
#include <numeric> // For std::accumulate
#include <cmath>   // For std::sqrt

// Protobuf 相关头文件
#include <ros/protobuffer_traits.h>
#include <ros/serialization_protobuffer.h>
#include "publish_info.pb.h" // 你的 Protobuf 消息头文件

// ROS 原生消息相关头文件 (如果也测试 ROS 原生消息的话)
#include <myproject/Person.h> // 你的 ROS 原生消息头文件

#define NUM_PUBLICATIONS 100 // 测试次数

int main(int argc, char** argv) {
    ros::init(argc, argv, "end_to_end_publisher_node");
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
                                   ros::console::levels::Debug);
    ros::NodeHandle nh;

    // --- Protobuf 发布器示例 ---
    ros::Publisher pb_pub = nh.advertise<pbtest::efftest>("/my_protobuf_topic", 1000);

    // --- ROS 原生消息发布器示例 ---
    ros::Publisher ros_native_pub = nh.advertise<myproject::Person>("/my_ros_native_topic", 1000);

    ros::Rate loop_rate(5); // 发布频率

    // Protobuf 消息实例
    pbtest::efftest proto_msg_to_publish;
    proto_msg_to_publish.set_name("Robotinho");
    proto_msg_to_publish.set_address("火星基地阿尔法");
    // proto_msg_to_publish.add_int_array(...); // 如果有复杂字段，在这里填充

    // ROS 原生消息实例
    myproject::Person ros_native_msg_to_publish;
    ros_native_msg_to_publish.name = "Original_Robotinho";
    ros_native_msg_to_publish.address = "Original_Mars_Base_Alpha";
    // ros_native_msg_to_publish.int_array.resize(...); // 如果有复杂字段，在这里填充

    std::vector<long long> proto_serialization_times_ns;
    std::vector<long long> ros_serialization_times_ns;
    
    int count = 0;
    while (ros::ok() && count < NUM_PUBLICATIONS) {
        // --- 发布 Protobuf 消息 ---
        proto_msg_to_publish.set_num(std::to_string(count));
        proto_msg_to_publish.set_send_timestamp_ns(ros::Time::now().toNSec()); // 填充时间戳！

        auto pb_start_serialize = std::chrono::high_resolution_clock::now();
        pb_pub.publish(proto_msg_to_publish);
        auto pb_end_serialize = std::chrono::high_resolution_clock::now();
        proto_serialization_times_ns.push_back(std::chrono::duration_cast<std::chrono::nanoseconds>(pb_end_serialize - pb_start_serialize).count());

        // --- 发布 ROS 原生消息 ---
        ros_native_msg_to_publish.num = std::to_string(count);
        ros_native_msg_to_publish.header.stamp = ros::Time::now(); // 填充时间戳！
        ros_native_msg_to_publish.header.seq = count; // 序列号，可选但推荐

        auto ros_start_serialize = std::chrono::high_resolution_clock::now();
        ros_native_pub.publish(ros_native_msg_to_publish);
        auto ros_end_serialize = std::chrono::high_resolution_clock::now();
        ros_serialization_times_ns.push_back(std::chrono::duration_cast<std::chrono::nanoseconds>(ros_end_serialize - ros_start_serialize).count());

        count++;

        ros::spinOnce();
        loop_rate.sleep();
    }

    // --- 打印 Protobuf 序列化结果 ---
    if (!proto_serialization_times_ns.empty()) {
        long long total = 0; for(long long t : proto_serialization_times_ns) total += t;
        double avg = static_cast<double>(total) / proto_serialization_times_ns.size();
        std::cout << "\n--- Protobuf 序列化平均时间: " << avg / 1000.0 << " us ---" << std::endl;
    }

    // --- 打印 ROS 原生序列化结果 ---
    if (!ros_serialization_times_ns.empty()) {
        long long total = 0; for(long long t : ros_serialization_times_ns) total += t;
        double avg = static_cast<double>(total) / ros_serialization_times_ns.size();
        std::cout << "\n--- ROS 原生序列化平均时间: " << avg / 1000.0 << " us ---" << std::endl;
    }

    return 0;
}

#endif

#if 0
#include <ros/ros.h>
#include <chrono>
#include <iostream>
#include <string>
#include <vector>
#include <numeric> // 用于 std::accumulate
#include <cmath>   // 用于 std::sqrt

// Protobuf 相关头文件
#include <ros/protobuffer_traits.h>
#include <ros/serialization_protobuffer.h>
#include "publish_info.pb.h" // 你的 Protobuf 消息头文件

// ROS 原生消息相关头文件
#include <myproject/Person.h> // 你的 ROS 原生消息头文件
#include <ros/serialization.h> // [新增]用于获取 ROS 原生消息的序列化长度

#define NUM_PUBLICATIONS 100 // 测试次数

int main(int argc, char** argv) {
    ros::init(argc, argv, "end_to_end_publisher_node");
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
                                   ros::console::levels::Debug);
    ros::NodeHandle nh;

    // --- Protobuf 发布器示例 ---
    ros::Publisher pb_pub = nh.advertise<pbtest::efftest>("/my_protobuf_topic", 1000);

    // --- ROS 原生消息发布器示例 ---
    ros::Publisher ros_native_pub = nh.advertise<myproject::Person>("/my_ros_native_topic", 1000);

    ros::Rate loop_rate(5); // 发布频率

    // Protobuf 消息实例
    pbtest::efftest proto_msg_to_publish;
    proto_msg_to_publish.set_name("Robotinho");
    proto_msg_to_publish.set_address("火星基地阿尔法");
    proto_msg_to_publish.set_num("19999999999");

    // 如果有复杂字段，在这里填充
    // proto_msg_to_publish.add_int_array(...);

    // ROS 原生消息实例
    myproject::Person ros_native_msg_to_publish;
    ros_native_msg_to_publish.name = "Original_Robotinho";
    ros_native_msg_to_publish.address = "Original_Mars_Base_Alpha";

    // 如果有复杂字段，在这里填充
    // ros_native_msg_to_publish.int_array.resize(...);

    std::vector<long long> proto_serialization_times_ns;
    std::vector<long long> ros_serialization_times_ns;

    long long total_pb_serialized_size = 0;   // 【新增】Protobuf 序列化后的总大小
    long long total_ros_serialized_size = 0; // 【新增】ROS 原生序列化后的总大小
    
    int count = 0;
    while (ros::ok() && count < NUM_PUBLICATIONS) {
        // --- 发布 Protobuf 消息 ---
        proto_msg_to_publish.set_num(std::to_string(count));
        proto_msg_to_publish.set_send_timestamp_ns(ros::Time::now().toNSec()); // 填充时间戳！

        // 【新增】获取 Protobuf 序列化后的大小
        uint32_t current_pb_serialized_size = proto_msg_to_publish.ByteSizeLong(); // C++11+ 使用 ByteSizeLong()
        total_pb_serialized_size += current_pb_serialized_size;

        auto pb_start_serialize = std::chrono::high_resolution_clock::now();
        pb_pub.publish(proto_msg_to_publish);
        auto pb_end_serialize = std::chrono::high_resolution_clock::now();
        proto_serialization_times_ns.push_back(std::chrono::duration_cast<std::chrono::nanoseconds>(pb_end_serialize - pb_start_serialize).count());

        // --- 发布 ROS 原生消息 ---
        ros_native_msg_to_publish.num = std::to_string(count);
        ros_native_msg_to_publish.header.stamp = ros::Time::now(); // 填充时间戳！
        ros_native_msg_to_publish.header.seq = count; // 序列号，可选但推荐

        // [新增]获取 ROS 原生消息序列化后的大小
        uint32_t current_ros_serialized_size = ros::serialization::serializationLength(ros_native_msg_to_publish);
        total_ros_serialized_size += current_ros_serialized_size;

        auto ros_start_serialize = std::chrono::high_resolution_clock::now();
        ros_native_pub.publish(ros_native_msg_to_publish);
        auto ros_end_serialize = std::chrono::high_resolution_clock::now();
        ros_serialization_times_ns.push_back(std::chrono::duration_cast<std::chrono::nanoseconds>(ros_end_serialize - ros_start_serialize).count());

        count++;

        ros::spinOnce();
        loop_rate.sleep();
    }

    // --- 打印 Protobuf 序列化结果 ---
    if (!proto_serialization_times_ns.empty()) {
        long long total_time = 0; for(long long t : proto_serialization_times_ns) total_time += t;
        double avg_time = static_cast<double>(total_time) / proto_serialization_times_ns.size();
        
        double avg_pb_serialized_size = static_cast<double>(total_pb_serialized_size) / proto_serialization_times_ns.size();

        std::cout << "\n--- Protobuf 性能测试结果 ---" << std::endl;
        std::cout << "总发布次数: " << NUM_PUBLICATIONS << std::endl;
        std::cout << "平均序列化耗时: " << avg_time / 1000.0 << " 微秒" << std::endl;
        std::cout << "**平均序列化后大小: " << avg_pb_serialized_size << " 字节**" << std::endl; // 强调这一行
        // 如果需要，可以添加时间标准差的计算
    }

    // --- 打印 ROS 原生序列化结果 ---
    if (!ros_serialization_times_ns.empty()) {
        long long total_time = 0; for(long long t : ros_serialization_times_ns) total_time += t;
        double avg_time = static_cast<double>(total_time) / ros_serialization_times_ns.size();

        double avg_ros_serialized_size = static_cast<double>(total_ros_serialized_size) / ros_serialization_times_ns.size();

        std::cout << "\n--- ROS 原生消息性能测试结果 ---" << std::endl;
        std::cout << "总发布次数: " << NUM_PUBLICATIONS << std::endl;
        std::cout << "平均序列化耗时: " << avg_time / 1000.0 << " 微秒" << std::endl;
        std::cout << "**平均序列化后大小: " << avg_ros_serialized_size << " 字节**" << std::endl; // 强调这一行
        // 如果需要，可以添加时间标准差的计算
    }

    return 0;
}

#endif

#if 0
#include <ros/ros.h>
#include <chrono>
#include <iostream>
#include <string>
#include <vector>
#include <numeric> // For std::accumulate
#include <cmath>   // For std::sqrt
#include <map>     // For simulating map in ROS native message

// Protobuf 相关头文件
#include <ros/protobuffer_traits.h>
#include <ros/serialization_protobuffer.h>
// 假设你有一个名为 'complex_data.proto' 的 Protobuf 文件，并已生成 'complex_data.pb.h'
#include "publish_info.pb.h" // 新的复杂 Protobuf 消息头文件

// ROS 原生消息相关头文件
// 假设你有一个名为 'ComplexSensorData.msg' 的 ROS 消息文件
#include <myproject/ComplexSensorData.h> // 新的复杂 ROS 原生消息头文件
#include <ros/serialization.h> // 用于获取 ROS 原生消息的序列化长度
#include <std_msgs/Header.h> // ROS Header for timestamp and frame_id
#include <geometry_msgs/Point.h> // For 3D points in ROS native message

#define NUM_PUBLICATIONS 100 // 测试次数
#define NUM_POINTS_IN_MESSAGE 100 // 消息中包含的 3D 点数量
#define NUM_MEASUREMENTS_IN_MESSAGE 200 // 消息中包含的测量值数量
#define NUM_METADATA_ENTRIES 10 // 消息中包含的元数据条目数量

int main(int argc, char** argv) {
    ros::init(argc, argv, "end_to_end_publisher_node");
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
                                   ros::console::levels::Debug);
    ros::NodeHandle nh;

    // --- Protobuf 发布器示例 ---
    // 假设你的 Protobuf 消息是 pbtest::ComplexSensorData
    ros::Publisher pb_pub = nh.advertise<pbtest::ComplexSensorData>("/my_protobuf_complex_topic", 1000);

    // --- ROS 原生消息发布器示例 ---
    // 假设你的 ROS 原生消息是 myproject::ComplexSensorData
    ros::Publisher ros_native_pub = nh.advertise<myproject::ComplexSensorData>("/my_ros_native_complex_topic", 1000);

    ros::Rate loop_rate(5); // 发布频率

    // Protobuf 消息实例
    pbtest::ComplexSensorData proto_msg_to_publish;
    // 填充固定字段
    proto_msg_to_publish.mutable_header()->set_frame_id("complex_sensor_frame");
    proto_msg_to_publish.set_sensor_name("Lidar_3D_Sensor");
    proto_msg_to_publish.set_temperature(25.5);

    // ROS 原生消息实例
    myproject::ComplexSensorData ros_native_msg_to_publish;
    // 填充固定字段
    ros_native_msg_to_publish.header.frame_id = "complex_sensor_frame";
    ros_native_msg_to_publish.sensor_name = "Lidar_3D_Sensor";
    ros_native_msg_to_publish.temperature = 25.5;

    std::vector<long long> proto_serialization_times_ns;
    std::vector<long long> ros_serialization_times_ns;

    long long total_pb_serialized_size = 0;   // Protobuf 序列化后的总大小
    long long total_ros_serialized_size = 0; // ROS 原生序列化后的总大小

    int count = 0;
    while (ros::ok() && count < NUM_PUBLICATIONS) {
        // --- 填充 Protobuf 消息的动态字段 ---
        proto_msg_to_publish.mutable_header()->set_seq(count);
        proto_msg_to_publish.mutable_header()->set_timestamp_ns(ros::Time::now().toNSec()); // 填充时间戳！

        // 清空并重新填充 repeated 字段以模拟动态数据
        proto_msg_to_publish.clear_points();
        for (int i = 0; i < NUM_POINTS_IN_MESSAGE; ++i) {
            pbtest::Point3D* point = proto_msg_to_publish.add_points();
            point->set_x(static_cast<double>(i) * 0.1 + std::sin(count * 0.05));
            point->set_y(static_cast<double>(i) * 0.2 + std::cos(count * 0.05));
            point->set_z(static_cast<double>(i) * 0.3 + std::tan(count * 0.05));
        }

        proto_msg_to_publish.clear_measurements();
        for (int i = 0; i < NUM_MEASUREMENTS_IN_MESSAGE; ++i) {
            proto_msg_to_publish.add_measurements(static_cast<float>(i) * 0.5 + std::sin(count * 0.1));
        }

        proto_msg_to_publish.clear_metadata();
        for (int i = 0; i < NUM_METADATA_ENTRIES; ++i) {
            proto_msg_to_publish.mutable_metadata()->insert({
                "key_" + std::to_string(i),
                "value_" + std::to_string(i) + "_iter_" + std::to_string(count)
            });
        }

        // 获取 Protobuf 序列化后的大小
        uint32_t current_pb_serialized_size = proto_msg_to_publish.ByteSizeLong();
        total_pb_serialized_size += current_pb_serialized_size;

        auto pb_start_serialize = std::chrono::high_resolution_clock::now();
        pb_pub.publish(proto_msg_to_publish);
        auto pb_end_serialize = std::chrono::high_resolution_clock::now();
        proto_serialization_times_ns.push_back(std::chrono::duration_cast<std::chrono::nanoseconds>(pb_end_serialize - pb_start_serialize).count());

        // --- 填充 ROS 原生消息的动态字段 ---
        ros_native_msg_to_publish.header.stamp = ros::Time::now(); // 填充时间戳！
        ros_native_msg_to_publish.header.seq = count; // 序列号，可选但推荐

        // 清空并重新填充动态数组
        ros_native_msg_to_publish.points.clear();
        ros_native_msg_to_publish.points.resize(NUM_POINTS_IN_MESSAGE);
        for (int i = 0; i < NUM_POINTS_IN_MESSAGE; ++i) {
            ros_native_msg_to_publish.points[i].x = static_cast<double>(i) * 0.1 + std::sin(count * 0.05);
            ros_native_msg_to_publish.points[i].y = static_cast<double>(i) * 0.2 + std::cos(count * 0.05);
            ros_native_msg_to_publish.points[i].z = static_cast<double>(i) * 0.3 + std::tan(count * 0.05);
        }

        ros_native_msg_to_publish.measurements.clear();
        ros_native_msg_to_publish.measurements.resize(NUM_MEASUREMENTS_IN_MESSAGE);
        for (int i = 0; i < NUM_MEASUREMENTS_IN_MESSAGE; ++i) {
            ros_native_msg_to_publish.measurements[i] = static_cast<float>(i) * 0.5 + std::sin(count * 0.1);
        }

        ros_native_msg_to_publish.metadata_keys.clear();
        ros_native_msg_to_publish.metadata_values.clear();
        ros_native_msg_to_publish.metadata_keys.resize(NUM_METADATA_ENTRIES);
        ros_native_msg_to_publish.metadata_values.resize(NUM_METADATA_ENTRIES);
        for (int i = 0; i < NUM_METADATA_ENTRIES; ++i) {
            ros_native_msg_to_publish.metadata_keys[i] = "key_" + std::to_string(i);
            ros_native_msg_to_publish.metadata_values[i] = "value_" + std::to_string(i) + "_iter_" + std::to_string(count);
        }

        // 获取 ROS 原生消息序列化后的大小
        uint32_t current_ros_serialized_size = ros::serialization::serializationLength(ros_native_msg_to_publish);
        total_ros_serialized_size += current_ros_serialized_size;

        auto ros_start_serialize = std::chrono::high_resolution_clock::now();
        ros_native_pub.publish(ros_native_msg_to_publish);
        auto ros_end_serialize = std::chrono::high_resolution_clock::now();
        ros_serialization_times_ns.push_back(std::chrono::duration_cast<std::chrono::nanoseconds>(ros_end_serialize - ros_start_serialize).count());

        count++;

        ros::spinOnce();
        loop_rate.sleep();
    }

    // --- 打印 Protobuf 序列化结果 ---
    if (!proto_serialization_times_ns.empty()) {
        long long total_time = 0; for(long long t : proto_serialization_times_ns) total_time += t;
        double avg_time = static_cast<double>(total_time) / proto_serialization_times_ns.size();

        double avg_pb_serialized_size = static_cast<double>(total_pb_serialized_size) / proto_serialization_times_ns.size();

        std::cout << "\n--- Protobuf 性能测试结果 ---" << std::endl;
        std::cout << "总发布次数: " << NUM_PUBLICATIONS << std::endl;
        std::cout << "平均序列化耗时: " << avg_time / 1000.0 << " 微秒" << std::endl;
        std::cout << "**平均序列化后大小: " << avg_pb_serialized_size << " 字节**" << std::endl;
        // 如果需要，可以添加时间标准差的计算
    } else {
        std::cout << "没有发布 Protobuf 消息进行测试。" << std::endl;
    }

    // --- 打印 ROS 原生序列化结果 ---
    if (!ros_serialization_times_ns.empty()) {
        long long total_time = 0; for(long long t : ros_serialization_times_ns) total_time += t;
        double avg_time = static_cast<double>(total_time) / ros_serialization_times_ns.size();

        double avg_ros_serialized_size = static_cast<double>(total_ros_serialized_size) / ros_serialization_times_ns.size();

        std::cout << "\n--- ROS 原生消息性能测试结果 ---" << std::endl;
        std::cout << "总发布次数: " << NUM_PUBLICATIONS << std::endl;
        std::cout << "平均序列化耗时: " << avg_time / 1000.0 << " 微秒" << std::endl;
        std::cout << "**平均序列化后大小: " << avg_ros_serialized_size << " 字节**" << std::endl;
        // 如果需要，可以添加时间标准差的计算
    } else {
        std::cout << "没有发布 ROS 原生消息进行测试。" << std::endl;
    }

    return 0;
}

#endif

#if 0
#include <ros/ros.h>
#include <chrono>
#include <iostream>
#include <string>
#include <vector>
#include <numeric>
#include <cmath>
#include <cstdlib>

// Protobuf
#include <ros/protobuffer_traits.h>
#include <ros/serialization_protobuffer.h>
#include "publish_info.pb.h"

// ROS 原生消息
#include <myproject/Person.h>
#include <myproject/SubSensor.h>
#include <ros/serialization.h>

#define NUM_PUBLICATIONS 100

int main(int argc, char** argv) {
    ros::init(argc, argv, "end_to_end_publisher_node");
    ros::NodeHandle nh;

    ros::Publisher pb_pub = nh.advertise<pbtest::efftest>("/my_protobuf_topic", 1000);
    ros::Publisher ros_pub = nh.advertise<myproject::Person>("/my_ros_native_topic", 1000);

    ros::Rate rate(5);

    std::vector<long long> pb_times, ros_times;
    long long pb_size = 0, ros_size = 0;

    for (int count = 0; ros::ok() && count < NUM_PUBLICATIONS; ++count) {
        // ---------------- Protobuf 填充 ----------------
        pbtest::efftest proto_msg;
        proto_msg.set_name("Robotinho");
        proto_msg.set_address("火星基地阿尔法");
        proto_msg.set_num(std::to_string(count));
        proto_msg.set_send_timestamp_ns(ros::Time::now().toNSec());

        for (int i = 0; i < 1000; ++i) proto_msg.add_int_array(i);
        for (int i = 0; i < 20; ++i) proto_msg.add_tags("tag_" + std::to_string(i));
        for (int i = 0; i < 50; ++i)
            (*proto_msg.mutable_score_map())["key_" + std::to_string(i)] = i * 0.1f;

        for (int i = 0; i < 10; ++i) {
            pbtest::SubMessage* sensor = proto_msg.add_sensors();
            sensor->set_sensor_name("sensor_" + std::to_string(i));
            sensor->set_timestamp_ns(ros::Time::now().toNSec());
            for (int j = 0; j < 100; ++j)
                sensor->add_readings(static_cast<float>(rand()) / RAND_MAX);
        }

        proto_msg.set_is_active(true);
        proto_msg.set_confidence(0.9876);

        pb_size += proto_msg.ByteSizeLong();

        auto pb_start = std::chrono::high_resolution_clock::now();
        pb_pub.publish(proto_msg);
        auto pb_end = std::chrono::high_resolution_clock::now();
        pb_times.push_back(std::chrono::duration_cast<std::chrono::nanoseconds>(pb_end - pb_start).count());

        // ---------------- ROS 原生填充 ----------------
        myproject::Person ros_msg;
        ros_msg.header.stamp = ros::Time::now();
        ros_msg.header.seq = count;
        ros_msg.name = "Original_Robotinho";
        ros_msg.address = "Original_Mars_Base_Alpha";
        ros_msg.num = std::to_string(count);

        for (int i = 0; i < 1000; ++i) ros_msg.int_array.push_back(i);
        for (int i = 0; i < 20; ++i) ros_msg.tags.push_back("tag_" + std::to_string(i));
        for (int i = 0; i < 50; ++i) {
            ros_msg.score_map_keys.push_back("key_" + std::to_string(i));
            ros_msg.score_map_values.push_back(i * 0.1f);
        }

        myproject::SubSensor sensor;
        for (int i = 0; i < 10; ++i) {
            sensor.sensor_name = "sensor_" + std::to_string(i);
            sensor.timestamp_ns = ros::Time::now().toNSec();
            sensor.readings.clear();
            for (int j = 0; j < 100; ++j)
                sensor.readings.push_back(static_cast<float>(rand()) / RAND_MAX);
            ros_msg.sensors.push_back(sensor);
        }

        ros_msg.is_active = true;
        ros_msg.confidence = 0.9876;

        ros_size += ros::serialization::serializationLength(ros_msg);

        auto ros_start = std::chrono::high_resolution_clock::now();
        ros_pub.publish(ros_msg);
        auto ros_end = std::chrono::high_resolution_clock::now();
        ros_times.push_back(std::chrono::duration_cast<std::chrono::nanoseconds>(ros_end - ros_start).count());

        ros::spinOnce();
        rate.sleep();
    }

    // ----------- 输出性能数据 -----------
    double pb_avg = std::accumulate(pb_times.begin(), pb_times.end(), 0LL) / 1000.0;
    double ros_avg = std::accumulate(ros_times.begin(), ros_times.end(), 0LL) / 1000.0;

    std::cout << "\n--- Protobuf 发布性能 ---\n";
    std::cout << "平均序列化耗时: " << pb_avg << " 微秒\n";
    std::cout << "平均序列化大小: " << pb_size / NUM_PUBLICATIONS << " 字节\n";

    std::cout << "\n--- ROS 原生发布性能 ---\n";
    std::cout << "平均序列化耗时: " << ros_avg << " 微秒\n";
    std::cout << "平均序列化大小: " << ros_size / NUM_PUBLICATIONS << " 字节\n";

    return 0;
}

#endif

#define TEST_MESSAGE_SIZE1 0
#if TEST_MESSAGE_SIZE1
#include <iostream>
#include <vector>
#include <string>
#include <ros/ros.h> // For ROS initialization (not strictly for serialization itself)
#include <ros/serialization.h> // For ROS serialization

#include "publish_info.pb.h"

// ROS 原生消息
#include <myproject/Person.h>

int main(int argc, char **argv) {
    ros::init(argc, argv, "serialization_comparer", ros::init_options::NoSigintHandler);

    std::cout << "--- Comparing Message Serialization Sizes ---" << std::endl;

    // --- 1. 准备数据 ---
    int test_id = 12345;
    std::string test_name = "Sensor_Data_Stream_From_Robot_Arm_Joint_1";
    std::vector<float> test_data_array;
    for (int i = 0; i < 100; ++i) { // 包含100个浮点数
        test_data_array.push_back(i * 0.1f);
    }

    // --- 2. 填充 ROS 消息 ---
    myproject::Person ros_msg;
    ros_msg.id = test_id;
    ros_msg.name = test_name;
    ros_msg.data_array = test_data_array;

    // --- 3. 填充 Protobuf 消息 ---
    pbtest::CompareMessage pb_msg;
    pb_msg.set_id(test_id);
    pb_msg.set_name(test_name);
    for (float val : test_data_array) {
        pb_msg.add_data_array(val);
    }

    // --- 4. 序列化 ROS 消息并测量大小 ---
    // ROS serialization requires a custom buffer or stream
    // We use ros::serialization::OStream to write to a buffer
    uint32_t ros_serialized_size = ros::serialization::serializationLength(ros_msg);
    std::vector<uint8_t> ros_buffer(ros_serialized_size);
    ros::serialization::OStream ros_os(ros_buffer.data(), ros_serialized_size);
    ros::serialization::serialize(ros_os, ros_msg);

    std::cout << "\nROS Message Size: " << ros_serialized_size << " bytes" << std::endl;

    // --- 5. 序列化 Protobuf 消息并测量大小 ---
    std::string pb_serialized_string;
    pb_msg.SerializeToString(&pb_serialized_string);

    std::cout << "Protobuf Message Size: " << pb_serialized_string.length() << " bytes" << std::endl;

    // --- 6. 额外测试：空数据或默认值的情况 ---
    std::cout << "\n--- Testing with Empty/Default Values ---" << std::endl;
    myproject::Person ros_empty_msg;
    ros_empty_msg.id = 0; // Default for int32
    ros_empty_msg.name = ""; // Default for string
    // data_array is empty by default

    pbtest::CompareMessage pb_empty_msg;
    // id and name are default (0 and empty string)
    // data_array is empty by default

    uint32_t ros_empty_serialized_size = ros::serialization::serializationLength(ros_empty_msg);
    // For Protobuf, if fields are default, they often aren't serialized
    std::string pb_empty_serialized_string;
    pb_empty_msg.SerializeToString(&pb_empty_serialized_string);

    std::cout << "ROS Empty Message Size: " << ros_empty_serialized_size << " bytes" << std::endl;
    std::cout << "Protobuf Empty Message Size: " << pb_empty_serialized_string.length() << " bytes" << std::endl;

    // Optional: Add a simple check for deserialization to ensure correctness
    // This is just for demonstration, not strictly for size comparison

    // CompareMessage deserialized_pb_msg;
    // if (deserialized_pb_msg.ParseFromString(pb_serialized_string)) {
    //     std::cout << "\nProtobuf deserialization successful: " << deserialized_pb_msg.name() << std::endl;
    // }

    return 0;
}

#endif


#define TEST_MESSAGE_SIZE2 1
#if TEST_MESSAGE_SIZE2
#include <iostream>
#include <vector>
#include <string>
#include <numeric> // For std::accumulate

#include <ros/ros.h>
#include <ros/serialization.h> // Required for ros::serialization::serializationLength
#include <ros/serialization_protobuffer.h>
// Protobuf 相关头文件
// 请确保这个路径正确，它应该是由 protoc 编译 .proto 文件后生成的
#include "publish_info.pb.h"

// ROS 原生消息相关头文件
// 请确保这个路径正确，它应该是由 catkin_make 编译 .msg 文件后生成的
#include <myproject/Person.h>

// 定义测试次数
#define NUM_PUBLICATIONS 100

int main(int argc, char** argv) {
    // 初始化 ROS 节点，不处理信号（如Ctrl+C），以便我们可以完成测试输出
    ros::init(argc, argv, "end_to_end_publisher_node", ros::init_options::NoSigintHandler);
    // 设置 ROS 控制台日志级别为 Debug，以便查看更多信息
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug);
    ros::NodeHandle nh; // ROS 节点句柄

    // --- Protobuf 消息发布器示例 (仅用于触发序列化，不实际发布) ---
    // 创建一个 Protobuf 消息的发布器，话题名为 "/my_protobuf_topic"，队列大小为 1000
    ros::Publisher pb_pub = nh.advertise<pbtest::CompareMessage>("/my_protobuf_topic", 1000); // 使用 CompareMessage

    // --- ROS 原生消息发布器示例 (仅用于触发序列化，不实际发布) ---
    // 创建一个 ROS 原生消息的发布器，话题名为 "/my_ros_native_topic"，队列大小为 1000
    ros::Publisher ros_native_pub = nh.advertise<myproject::Person>("/my_ros_native_topic", 1000);

    // 设置循环频率为 5 Hz (在此场景下不严格必要，但保留结构)
    ros::Rate loop_rate(5);

    // Protobuf 消息实例，用于发布
    pbtest::CompareMessage proto_msg_to_publish; // 使用 CompareMessage
    
    // 准备 Protobuf 消息的固定数据
    int test_id = 54321;
    // 增加字符串长度，以更明显地体现差距
    std::string test_name = "Robot_Model_X_Common_Name_With_A_Very_Long_Description_To_Fill_Up_More_Bytes_For_Comparison_Purposes_This_String_Is_Intentionally_Made_Longer_Than_Typical_To_Exaggerate_The_Size_Difference_Between_Serialization_Methods.";
    std::string test_email = "common.model.x.long.email.address.for.testing@example.com.long.domain.name.test";
    std::string test_address = "Common Labs, Test City, TC 12345, Building A, Floor 10, Room 1001, This_Is_A_Very_Long_Address_String_To_Demonstrate_Serialization_Efficiency_Across_Different_Protocols.";
    
    std::vector<float> test_data_array;
    // 增加浮点数数组大小，从 50 增加到 500 个浮点数
    for (int i = 0; i < 500; ++i) { 
        test_data_array.push_back(i * 0.5f + 0.123f);
    }

    std::vector<int32_t> test_int_array;
    // 整数数组，包含 1000 个小整数，以利用 Protobuf 的 varint 编码优势
    for (int i = 0; i < 1000; ++i) {
        test_int_array.push_back(i % 100); // 使用小整数，例如 0-99
    }

    // 新增数据类型的数据
    double test_double_value = 12345.6789012345;
    bool test_boolean_flag = true;
    uint64_t test_large_int_value = 9876543210987654321ULL; // 较大的 uint64 值
    
    std::vector<double> test_double_array;
    // 双精度浮点数数组，包含 200 个元素
    for (int i = 0; i < 200; ++i) {
        test_double_array.push_back(static_cast<double>(i) / 7.0 + 0.001);
    }

    // 新增的固定大小整数类型
    int8_t test_int8_value = -123;
    uint8_t test_uint8_value = 250;
    int16_t test_int16_value = -32000;
    uint16_t test_uint16_value = 65000;
    int64_t test_int64_value = -987654321098765432LL;

    // 新增的整数数组
    std::vector<int8_t> test_int8_array;
    for (int i = 0; i < 500; ++i) { // 500个 int8
        test_int8_array.push_back(static_cast<int8_t>(i % 128));
    }
    std::vector<uint16_t> test_uint16_array;
    for (int i = 0; i < 500; ++i) { // 500个 uint16
        test_uint16_array.push_back(static_cast<uint16_t>(i % 65536));
    }
    std::vector<int64_t> test_int64_array;
    for (int i = 0; i < 100; ++i) { // 100个 int64
        test_int64_array.push_back(static_cast<int64_t>(i) * 1000000000LL);
    }


    // ROS 原生消息实例，用于发布
    myproject::Person ros_native_msg_to_publish;
    
    // 存储序列化后总大小的变量
    long long total_pb_serialized_size = 0;
    long long total_ros_serialized_size = 0;

    int count = 0; // 循环计数器
    // 循环发布消息，直到 ROS 关闭或达到指定发布次数
    while (ros::ok() && count < NUM_PUBLICATIONS) {
        // --- 准备 Protobuf 消息 ---
        proto_msg_to_publish.set_id(test_id);
        proto_msg_to_publish.set_name(test_name);
        proto_msg_to_publish.set_email(test_email);
        proto_msg_to_publish.set_address(test_address);
        // 清空并重新填充 data_array，确保每次循环数据相同
        proto_msg_to_publish.clear_data_array();
        for (float val : test_data_array) {
            proto_msg_to_publish.add_data_array(val);
        }
        // 清空并重新填充 int_array
        proto_msg_to_publish.clear_int_array();
        for (int32_t val : test_int_array) {
            proto_msg_to_publish.add_int_array(val);
        }
        // 设置新增数据类型字段
        proto_msg_to_publish.set_double_value(test_double_value);
        proto_msg_to_publish.set_boolean_flag(test_boolean_flag);
        proto_msg_to_publish.set_large_int_value(test_large_int_value);
        proto_msg_to_publish.clear_double_array();
        for (double val : test_double_array) {
            proto_msg_to_publish.add_double_array(val);
        }
        // 设置新增的固定大小整数类型
        proto_msg_to_publish.set_int8_value(test_int8_value);
        proto_msg_to_publish.set_uint8_value(test_uint8_value);
        proto_msg_to_publish.set_int16_value(test_int16_value);
        proto_msg_to_publish.set_uint16_value(test_uint16_value);
        proto_msg_to_publish.set_int64_value(test_int64_value);
        // 清空并重新填充新增的整数数组
        proto_msg_to_publish.clear_int8_array();
        for (int8_t val : test_int8_array) {
            proto_msg_to_publish.add_int8_array(val);
        }
        proto_msg_to_publish.clear_uint16_array();
        for (uint16_t val : test_uint16_array) {
            proto_msg_to_publish.add_uint16_array(val);
        }
        proto_msg_to_publish.clear_int64_array();
        for (int64_t val : test_int64_array) {
            proto_msg_to_publish.add_int64_array(val);
        }

        // num 字段会在循环中动态设置，以确保每次消息内容略有不同
        proto_msg_to_publish.set_num(std::to_string(count));

        // 获取 Protobuf 消息序列化后的大小
        // ByteSizeLong() 返回序列化后消息的字节数
        uint32_t current_pb_serialized_size = proto_msg_to_publish.ByteSizeLong();
        total_pb_serialized_size += current_pb_serialized_size;

        // 实际发布消息 (即使不测量时间，也需要调用 publish 来模拟完整流程)
        pb_pub.publish(proto_msg_to_publish);

        // --- 准备 ROS 原生消息 ---
        ros_native_msg_to_publish.id = test_id; // 设置 ROS 消息的 id
        ros_native_msg_to_publish.name = test_name; // 使用相同的长字符串
        ros_native_msg_to_publish.email = test_email; // 使用相同的长字符串
        ros_native_msg_to_publish.address = test_address; // 使用相同的长字符串
        ros_native_msg_to_publish.data_array = test_data_array; // 使用相同大小的浮点数数组
        ros_native_msg_to_publish.int_array = test_int_array; // 使用相同大小的整数数组
        // 设置新增数据类型字段
        ros_native_msg_to_publish.double_value = test_double_value;
        ros_native_msg_to_publish.boolean_flag = test_boolean_flag;
        ros_native_msg_to_publish.large_int_value = test_large_int_value;
        ros_native_msg_to_publish.double_array = test_double_array;
        // 设置新增的固定大小整数类型
        ros_native_msg_to_publish.int8_value = test_int8_value;
        ros_native_msg_to_publish.uint8_value = test_uint8_value;
        ros_native_msg_to_publish.int16_value = test_int16_value;
        ros_native_msg_to_publish.uint16_value = test_uint16_value;
        ros_native_msg_to_publish.int64_value = test_int64_value;
        // 设置新增的整数数组
        ros_native_msg_to_publish.int8_array = test_int8_array;
        ros_native_msg_to_publish.uint16_array = test_uint16_array;
        ros_native_msg_to_publish.int64_array = test_int64_array;


        // 动态设置 num 字段
        ros_native_msg_to_publish.num = std::to_string(count);
        // 填充 ROS 消息头中的时间戳和序列号 (注意：Person.msg 中不再有 header 字段，这里需要移除)
        // ros_native_msg_to_publish.header.stamp = ros::Time::now();
        // ros_native_msg_to_publish.header.seq = count;

        // 获取 ROS 原生消息序列化后的大小
        // ros::serialization::serializationLength 计算消息序列化后的长度
        uint32_t current_ros_serialized_size = ros::serialization::serializationLength(ros_native_msg_to_publish);
        total_ros_serialized_size += current_ros_serialized_size;

        // 实际发布消息 (即使不测量时间，也需要调用 publish 来模拟完整流程)
        ros_native_pub.publish(ros_native_msg_to_publish);

        count++; // 增加计数器

        ros::spinOnce(); // 处理 ROS 回调
        loop_rate.sleep(); // 按照设定的频率休眠
    }

    // --- 打印 Protobuf 序列化大小结果 ---
    if (NUM_PUBLICATIONS > 0) {
        // 计算平均序列化后大小（字节）
        double avg_pb_serialized_size = static_cast<double>(total_pb_serialized_size) / NUM_PUBLICATIONS;

        std::cout << "\n--- Protobuf 消息大小测试结果 ---" << std::endl;
        std::cout << "总发布次数: " << NUM_PUBLICATIONS << std::endl;
        std::cout << "**平均序列化后大小: " << avg_pb_serialized_size << " 字节**" << std::endl;
    }

    // --- 打印 ROS 原生消息序列化大小结果 ---
    if (NUM_PUBLICATIONS > 0) {
        // 计算平均序列化后大小（字节）
        double avg_ros_serialized_size = static_cast<double>(total_ros_serialized_size) / NUM_PUBLICATIONS;

        std::cout << "\n--- ROS 原生消息大小测试结果 ---" << std::endl;
        std::cout << "总发布次数: " << NUM_PUBLICATIONS << std::endl;
        std::cout << "**平均序列化后大小: " << avg_ros_serialized_size << " 字节**" << std::endl;
    }

    return 0;
}



#endif